
#include "HrkAnim.h"

#pragma warning (disable:4996 4267)

	template <typename T>
	int CHrkKeyTab<T>::FindClosestKeyIndex( int frame,float t )
	{
		int a = 0;
		int b = m_keyCount - 1;

		int i = 0;

		while( 1 )
		{
			if( b - a == 1)
			{
				return a;
			}

			keyDef_t<T>* pKey1 =  (keyDef_t<T>*)&(_Myfirst[a]);
			
			if(pKey1->frame_index == frame )
			{
				return a;
			}

			keyDef_t<T>* pKey2 =  (keyDef_t<T>*)&(_Myfirst[b]);

			if(pKey2->frame_index == frame )
			{
				return b;
			}

			i = ( a + b )/2;

			keyDef_t<T>* pKey =  (keyDef_t<T>*)&(_Myfirst[i]);

			if( pKey->frame_index == frame )
			{
				return i;
			}

			if( pKey->frame_index < frame )
			{
				a = i;
				continue;
			}
			
			if( pKey->frame_index > frame )
			{
				b = i;
				continue;
			}

		}
	}
	
	template <typename T>
	bool CHrkKeyTab<T>::HaveAnimKeys()
	{
		return m_keyCount>1? true:false;
	}

	CHrkAnim::~CHrkAnim()
	{
		for (int i = 0; i < (int)m_nodeAnims.size(); i++) {
			if (m_nodeAnims[i] != NULL) {
				delete m_nodeAnims[i];
			}
		}
	}

	void CHrkAnim::Destroy()
	{
	}

	bool CHrkAnim::LoadFromFile( const char* filename )
	{
		char fn[MAX_PATH];

		strcpy(fn,filename);

		char* pdot = strrchr(fn,'.');
		if(pdot)
		{
			strcpy( pdot,".ANM");
		}

		TiXmlDocument doc(fn);
		bool loadOkay = doc.LoadFile();
		if (loadOkay == false) {
			return false;
		}

		TiXmlElement* rootElem = doc.RootElement();

		const TiXmlElement *elem = NULL;
		const char* s;

		int nNodeCount = 0;


		elem = rootElem->FirstChildElement("AnimNodes");

		if( !elem )
		{
			return false;
		}

		s = elem->Attribute("node_count");

		if( s )
		{
			sscanf( s,"%d",&nNodeCount);
		}

		if( nNodeCount == 0)
		{
			return false;
		}
		
		m_nNodeCount = nNodeCount;

		s = elem->Attribute("frame_count");

		if( s )
		{
			sscanf( s,"%d",&m_nFrameCount);
		}

		s = elem->Attribute("frame_rate");

		if( s )
		{
			sscanf( s,"%d",&m_nFrameRate);
		}

		s = elem->Attribute("frame_tick");
		
		if( s )
		{
			sscanf( s,"%f",&m_fFrameTick);
		}

#if 1	//test!!!
		m_fFrameTick = 1.0f / m_nFrameRate;
#endif

		elem = rootElem->FirstChildElement("Node");

		m_topName.clear();
		
		while(elem)
		{
			const char* name = elem->Attribute("name");
			m_topName.push_back (name );
			elem = elem->NextSiblingElement("Node");
		}

		int nTopName = (int)m_topName.size();
		
		m_bBiped = false;

		for( int i = 0;i< nTopName;i++ )
		{
			if( strcmp( m_topName[i].c_str(),"Bip01" ) == 0)
			{
				m_bBiped = true;
			}
		}

		elem = rootElem->FirstChildElement("Node");

		while(elem)
		{
			CHrkNodeAnim* pAnim = new CHrkNodeAnim(this);

			const char* name = elem->Attribute("name");

			pAnim->m_pNode = FindNode(name,m_nodeRoot);

			pAnim->LoadNodeAnim(elem);

			m_nodeAnims.push_back(pAnim);

			elem = elem->NextSiblingElement("Node");
		}

		//adjust Bip01 translation relative to Ground
		if( m_bBiped )
		{
			CHrkNodeAnim*bip01 = FindNodeAnim("Bip01");
			CHrkNodeAnim*ground = FindNodeAnim("Ground");
			CHrkNodeAnim *bone_arrow = FindNodeAnim("W_Bone_Arrow");

			bool bSubstract = true;

			m_BipRotInv = GLvec3f(0, 0, 0);

			if( ground ) {
				int n1 = bip01->m_posKeys[1].size();
				//int n2 = ground->m_posKeys[1].size();

				for( int j = 0;j<n1;j++) {			
					bip01->m_posKeys[0][j].val -= ground->m_posKeys[0][0].val;
					bip01->m_posKeys[1][j].val -= ground->m_posKeys[1][0].val;
					bip01->m_posKeys[2][j].val -= ground->m_posKeys[2][0].val;
				}

				if (bone_arrow) {
					int n = bone_arrow->m_posKeys[1].size();
					for( int j = 0;j<n1;j++) {			
						bone_arrow->m_posKeys[0][j].val -= ground->m_posKeys[0][0].val;
						bone_arrow->m_posKeys[1][j].val -= ground->m_posKeys[1][0].val;
						bone_arrow->m_posKeys[2][j].val -= ground->m_posKeys[2][0].val;
					}

				}

				if (ground->m_rotKeysQ.size() > 0) {
					ground->m_rotKeysQ[0].val.ToEuler(m_BipRotInv.x,m_BipRotInv.y,m_BipRotInv.z);
					m_BipRotInv.Negative();
				}

				//bip01->m_pNode->m_translate1 = bip01->m_pNode->m_translate1 - ground->m_pNode->m_translate1;
			}
			else {
				int j = 0;
			}
		}

		return true;
	}

	CHrkNode* CHrkAnim::FindNode(const char* nodeName, CHrkNode *pNode)
	{	
		CHrkNode* ret = NULL;

		if (strcmp(pNode->m_name.c_str(), nodeName) == 0) {
			return pNode;
		}

		int nChilds = pNode->m_child_list.Count();
		for( int i = 0;i<nChilds;i++ ){
			ret = FindNode( nodeName, pNode->m_child_list[i]);
			if( ret ){
				break;
			}
		}

		return ret;
	}

	void CHrkAnim::UpdateAnim(float t)
	{
		int frame = 0;

		float dt = FindClosestFrameIndex(t,frame );
		
		for( int i = 0;i<m_nNodeCount;i++)
		{
			m_nodeAnims[i]->UpdateNodeAnim(frame,dt, m_BipRotInv);
		}
	}

	float CHrkAnim::FindClosestFrameIndex( float t,int& index, float frame_tick)
	{
		if (frame_tick == 0.0f) {
			frame_tick = m_fFrameTick;
		}

		if( t< 0.f)
		{
			index = 0;
			return 0.f;
		}

		float ft = t/frame_tick;

		float ft1 = floor(ft);
		float ft2 = ceil(ft);

		float dt1 = ft - ft1;
		float dt2 = ft2 - ft;


		if( dt1 <= TIME_EPSILON )
		{
			index = (int) ft1;
			return 0.f;
			
		}
		else if( dt2 <= TIME_EPSILON )
		{
			index = (int) ft2;
			return 0.f;
		}
		else
		{
			index = (int) ft1;
			return dt1*frame_tick;
		}

		if( index >= m_nFrameCount )
		{
			index = m_nFrameCount - 1;
			return 0.f;
		}

		index = 0;
		return 0.f;
	}

	void CHrkNodeAnim::LoadNodeAnim( const TiXmlElement* elem )
	{
		const TiXmlElement* elem1 = elem;

		elem1 = elem->FirstChildElement("key_tab");
		
		const char* s = NULL;

		while(elem1)
		{
			InterpMode intp_mode = eInterp_UNKOWN;

			int nKeyCount = 0;

			s = elem1->Attribute("count");

			sscanf(s,"%d",&nKeyCount);

			s = elem1->Attribute("type");

			int t = 0;

			if(strcmp(s,"rot_q") == 0)
			{
				t = 1;
				m_rotKeysQ.m_interpMode = intp_mode;
				m_rotKeysQ.m_keyCount = nKeyCount;
				m_rotKeysQ.m_pGroupAnim = this;
			}
			else if(strcmp(s,"pos_x") == 0)
			{
				t = 2;
				m_posKeys[0].m_interpMode = intp_mode;
				m_posKeys[0].m_keyCount = nKeyCount;
				m_posKeys[0].m_pGroupAnim = this;
			}
			else if(strcmp(s,"pos_y") == 0)
			{
				t = 3;
				m_posKeys[1].m_interpMode = intp_mode;
				m_posKeys[1].m_keyCount = nKeyCount;
				m_posKeys[1].m_pGroupAnim = this;
			}
			else if(strcmp(s,"pos_z") == 0)
			{
				t = 4;
				m_posKeys[2].m_interpMode = intp_mode;
				m_posKeys[2].m_keyCount = nKeyCount;
				m_posKeys[2].m_pGroupAnim = this;
			}

			const char* sText = elem1->GetText();

			char* s1 = const_cast<char*> (sText);

//			float fval[4];

			for( int i = 0;i<nKeyCount;i++)
			{
				if( i == 0 )
				{
					s1 = strtok( s1,";");
				}
				else
				{
					s1 = strtok( NULL,";");
				}

				int nFrameIndex = 0;

				sscanf(s1,"%d",&nFrameIndex );

				s1 = strtok( NULL,";");

				if( t == 1)
				{
					quatKeyDef_t qk;
					sscanf( s1,"%f,%f,%f,%f",&qk.val.x,&qk.val.y,&qk.val.z,&qk.val.w);
					qk.frame_index = nFrameIndex;
					m_rotKeysQ.push_back( qk );
					
				}
				else if( t == 2 )
				{		
					floatKeyDef_t fk;
					sscanf( s1,"%f",&fk.val);
					fk.frame_index = nFrameIndex;
					m_posKeys[0].push_back(fk);
				}
				else if( t == 3 )
				{	
					floatKeyDef_t fk;
					sscanf( s1,"%f",&fk.val);
					fk.frame_index = nFrameIndex;
					m_posKeys[1].push_back(fk);
				}
				else if( t == 4 )
				{
					floatKeyDef_t fk;
					sscanf( s1,"%f",&fk.val);
					fk.frame_index = nFrameIndex;
					m_posKeys[2].push_back(fk);
				}

			}

			elem1 = elem1->NextSiblingElement("key_tab");
		}		
	}


	void CHrkNodeAnim::UpdateNodeAnim( int frame,float t, const GLvec3f &BipRotInv)
	{
		if (m_pNode == NULL)
			return;

		int key_index = 0;

		m_pNode->m_rotate = m_pNode->m_rotate1;
		m_pNode->m_translate = m_pNode->m_translate1;
		m_pNode->m_scale = m_pNode->m_scale1;

		if(m_rotKeysQ.HaveAnimKeys()) {
			key_index = m_rotKeysQ.FindClosestKeyIndex( frame,t);
			m_rotKeysQ.InterpolateKeyframe(key_index,frame,(int)t);
			
			GLvec3f rotate;

			m_rotKeysQ.m_valInterp.ToEuler(rotate.x,rotate.y,rotate.z);

			m_pNode->m_rotate = rotate;
		}

		if( m_posKeys[0].HaveAnimKeys()) {
			key_index = m_posKeys[0].FindClosestKeyIndex( frame,t);
			m_posKeys[0].InterpolateKeyframe(key_index,frame,(int)t);

			m_pNode->m_translate.x = m_posKeys[0].m_valInterp;
		}

		if( m_posKeys[1].HaveAnimKeys()) {
			key_index = m_posKeys[1].FindClosestKeyIndex( frame,t);
			m_posKeys[1].InterpolateKeyframe(key_index,frame,(int)t);

			m_pNode->m_translate.y = m_posKeys[1].m_valInterp;
		}

		if( m_posKeys[2].HaveAnimKeys()) {
			key_index = m_posKeys[2].FindClosestKeyIndex( frame,t);
			m_posKeys[2].InterpolateKeyframe(key_index,frame,(int)t);

			m_pNode->m_translate.z = m_posKeys[2].m_valInterp;
		}

		//m_pNode->m_mat.Compose( m_pNode->m_translate,m_pNode->m_rotate,m_pNode->m_scale);

#if 0
		if( m_pNode->m_parent->m_name.compare("GROUP_ROOT")==0 ) {
			if (!BipRotInv.IsEqual( GLvec3f(0, 0, 0))) {
				GLmat4f matRotation, m1, m2;

				matRotation.MakeRotateX( BipRotInv.x);
				m1.MakeRotateY( BipRotInv.y);
				matRotation.Mult( m1 );
				m2.MakeRotateZ( BipRotInv.z);
				matRotation.Mult( m2 );

				m_pNode->m_mat.Mult( matRotation );
			}
		}
#endif

	}

	bool CHrkNodeAnim::HaveAnim()
	{
		if( m_rotKeysQ.HaveAnimKeys() )
			return true;
		if( m_posKeys[0].HaveAnimKeys() )
			return true;
		if( m_posKeys[1].HaveAnimKeys() )
			return true;
		if( m_posKeys[2].HaveAnimKeys() )
			return true;
		return false;
	}

	bool CHrkNodeAnim::HavePosAnim()
	{
		if( m_posKeys[0].HaveAnimKeys() )
			return true;
		if( m_posKeys[1].HaveAnimKeys() )
			return true;
		if( m_posKeys[2].HaveAnimKeys() )
			return true;
		return false;
	}
	bool CHrkNodeAnim::HaveRotAnim()
	{
		if( m_rotKeysQ.HaveAnimKeys() )
			return true;
		return false;
	}

	CHrkNodeAnim* CHrkAnim::FindNodeAnim( const char* name )
	{
		int c = (int)m_nodeAnims.size();
		for( int i = 0;i<c;i++ )
		{
			if (m_nodeAnims[i]->m_pNode != NULL) {
				if( strcmp( m_nodeAnims[i]->m_pNode->m_name.c_str(),name ) == 0)
				{
					return m_nodeAnims[i];
				}
			}
		}

		return NULL;
		
	}

	void CHrkAnim::SetBipedAnimation( const char* name )
	{
		CHrkNodeAnim* groupAnim1 = FindNodeAnim( "Bip01" );
		CHrkNodeAnim* groupAnim2 = FindNodeAnim( name );

		if( !groupAnim1->HavePosAnim())
		{

			int nk = (int)groupAnim2->m_posKeys[0].size();

			if( nk > 0)
			{
				groupAnim1->m_posKeys[0] = groupAnim2->m_posKeys[0];

				for( int i = 0;i<nk;i ++ )
				{
					groupAnim1->m_posKeys[0][i].val -= groupAnim2->m_posKeys[0][0].val;
					groupAnim1->m_posKeys[0][i].val += groupAnim1->m_pNode->m_translate.x;
				}
				groupAnim1->m_posKeys[0].m_keyCount = nk;
			}

			nk = (int)groupAnim2->m_posKeys[1].size();

			if( nk > 0)
			{
				groupAnim1->m_posKeys[1] = groupAnim2->m_posKeys[1];

				for( int i = 0;i<nk;i ++ )
				{
					groupAnim1->m_posKeys[1][i].val -= groupAnim2->m_posKeys[1][0].val;
					groupAnim1->m_posKeys[1][i].val += groupAnim1->m_pNode->m_translate.y;
				}

				groupAnim1->m_posKeys[1].m_keyCount = nk;
			}

			nk = (int)groupAnim2->m_posKeys[2].size();

			if( nk > 0)
			{
				groupAnim1->m_posKeys[2] = groupAnim2->m_posKeys[2];

				for( int i = 0;i<nk;i ++ )
				{
					float fval = groupAnim1->m_posKeys[2][i].val - groupAnim2->m_posKeys[2][0].val;
					fval +=  groupAnim1->m_pNode->m_translate.z;

					groupAnim1->m_posKeys[2][i].val = fval;
					
				}
				groupAnim1->m_posKeys[2].m_keyCount = nk;
			}
		}
		
	}

	bool CHrkAnim::CheckLoop()
	{
		int c = (int)m_nodeAnims.size();
		for( int i = 0;i<c;i++ ) {
			CHrkNodeAnim* pGroupAnim = m_nodeAnims[i];
			if (pGroupAnim->m_pNode != NULL) {
				if (pGroupAnim->m_pNode->m_name.compare("Bip01")!=0 && pGroupAnim->m_pNode->m_name.compare("Ground")!=0) {
					if (pGroupAnim->HavePosAnim()) {
						if (!pGroupAnim->m_posKeys[0].IsLoop(pGroupAnim->m_posKeys[0].size()) ||
							!pGroupAnim->m_posKeys[1].IsLoop(pGroupAnim->m_posKeys[1].size()) ||
							!pGroupAnim->m_posKeys[2].IsLoop(pGroupAnim->m_posKeys[2].size())) {
								return false;
						}
					}

					if (pGroupAnim->HaveRotAnim()) {
						if (!pGroupAnim->m_rotKeysQ.IsLoop(pGroupAnim->m_rotKeysQ.size())) {
							return false;
						}

					}
				}
			}

		}

		return true;
	}

	void CHrkQuatKeyTab::InterpolateKeyframe( int keyIndex,int frame,int t )
	{
		quatKeyDef_t* k = (quatKeyDef_t*)&_Myfirst[keyIndex];

		if( k->frame_index == frame && t < TIME_EPSILON )
		{
			m_valInterp = k->val;
		}
		else
		{
			quatKeyDef_t* k1 = k+1;
			float frame_tick = m_pGroupAnim->m_pAnim->m_fFrameTick;

			float kt = (( frame - k->frame_index ) * frame_tick + t)/((k1->frame_index - k->frame_index)*frame_tick);

			m_valInterp.Slerp( k->val,k1->val,kt);

		}
	}

	void CHrkFloatKeyTab::InterpolateKeyframe( int keyIndex,int frame,int t )
	{
		floatKeyDef_t* k = (floatKeyDef_t*)&_Myfirst[keyIndex];

		if( k->frame_index == frame && t < TIME_EPSILON )
		{
			m_valInterp = k->val;
		}
		else
		{
			floatKeyDef_t* k1 = k+1;
			float frame_tick = m_pGroupAnim->m_pAnim->m_fFrameTick;

			float kt = (( frame - k->frame_index ) * frame_tick + t)/((k1->frame_index - k->frame_index)*frame_tick);

			m_valInterp = (1.f-kt)*k->val + kt*k1->val;
		}
	}

	bool CHrkQuatKeyTab::IsLoop(int frameCount)
	{
		quatKeyDef_t* k_first = (quatKeyDef_t*)&_Myfirst[0];
		quatKeyDef_t* k_end = (quatKeyDef_t*)&_Myfirst[frameCount-1];

		GLquat q_first = k_first->val;
		GLquat q_end = k_end->val;

		return (FIS_EQ2(q_first.x, q_end.x) && FIS_EQ2(q_first.y, q_end.y) && FIS_EQ2(q_first.z, q_end.z)
			&& FIS_EQ2(q_first.w, q_end.w));
	}

	bool CHrkFloatKeyTab::IsLoop(int frameCount)
	{
		floatKeyDef_t* k_first = (floatKeyDef_t*)&_Myfirst[0];
		floatKeyDef_t* k_end = (floatKeyDef_t*)&_Myfirst[frameCount-1];

		float v_first = k_first->val;
		float v_end = k_end->val;

		return (FIS_EQ2(v_first, v_end));
	}